#include <iostream>
#include <vector>
#include <random>
#include <math.h>
#include <iterator>
#include <ctime>

using namespace std;

struct Pointcell{
    Pointcell():x_(-1), z_(-1), father_id_(-1), id_(-1) {}
    Pointcell(int xx, int zz):x_(xx), z_(zz), father_id_(-1), id_(-1) {}
    Pointcell(int xx, int zz, int father_id, int id):
    x_(xx), z_(zz), father_id_(father_id), id_(id) {}
    int x_, z_;
    int father_id_;
    int id_;
};

class Cplanning
{
public:
    Cplanning(int sizez, int sizex)
    {
        valid_world.resize(sizez);
        for(int i=0;i<sizez;i++){
            valid_world[i].resize(sizex);
        }
        prob_ = 0.80;//目标点采样概率
        iteration_ = 100000;//迭代次数
        map_max_x_ = sizex;
        map_max_z_ = sizez;//地图边界,网格分辨率0.100m
        point_list_.clear();
        path_.clear();//清空
        number_ = 1;
        iteration_number_ = 0;
        path_length_ = 0.0;
        srand((int)time(0));
        start_.x_ = sizex/2;
        start_.z_ = 1;
        goal_.x_ = start_.x_;
        goal_.z_ = sizez-45;
    }
    ~Cplanning() {}

    void motion_plan(){

        path_.clear();
        point_list_.clear();
        path_length_ = 0.0;
        iteration_number_ = 0;
        number_ = 1;
        if(valid_world[goal_.z_][goal_.x_]!=0){
            goal_.z_ = 49;
            while(goal_.z_>30 && goal_.z_<50){
                if(valid_world[goal_.z_][goal_.x_]!=0){
                    goal_.z_--;
                }
                else{
                    break;
                }
            }
        }
        if(valid_world[goal_.z_][goal_.x_]!=0){
            cout<<"目标为障碍物！无法到达！"<<endl;
            return;
        }

        start_.id_ = 0;
        start_.father_id_ = -1;
        point_list_.push_back(start_);
        
        if (begin_plan())
        {        
            cout<<"规划成功！"<<endl;
            get_path_and_length();
        }
        else{
            cout<<point_list_.size()<<endl;
            cout<<"规划失败！"<<endl;
        }
        //清理地图
        for(int i=0;i<valid_world.size();i++){
            for(int j=0;j<valid_world[0].size();j++){
                valid_world[i][j] = 0;
            }
        }

    }

    vector<Pointcell> point_list_;//路径点列表
    vector<Pointcell> path_;//多次迭代路径点
    vector<vector<int>> valid_world;
    int iteration_;//迭代次数
    int iteration_number_;//已经迭代次数
    double path_length_;//路径长度
    int map_max_x_;
    int map_max_z_;//地图边界
    int number_;//编号
    double prob_;//目标点采样概率
    Pointcell start_;//起始点
    Pointcell goal_;//结束点

    bool begin_plan(){
        int x;
        int z;
        for(int i=0; i<iteration_; i++){
            if(rand()%11/10.0 < prob_){
                x = goal_.x_;
                z = goal_.z_;
            }
            else{
                x = rand()%map_max_x_;
                z = rand()%map_max_z_;
            }
            //cout<<x<<" "<<y<<" "<<z<<" "<<td<<" "<<endl;
            iteration_number_ = i;
            if(push_point(x, z)){
                return true;
            }
        }
        return false;
    }

    void plot_tree(){
        for(int i=0;i<point_list_.size(); i++){
            //cout<<path_[i].x_<<"  "<<path_[i].y_<<endl;
            cout<<point_list_[i].x_<<"  "<<point_list_[i].z_<<endl;
        }
    }

    bool push_point(int x, int z){
        int cur_distance = 0;
        int min_distance = 99999999;
        int father_x, father_z, father_id;
        int index = 0;
        for(int i=0; i<point_list_.size(); i++){
            cur_distance = abs(x-point_list_[i].x_) + abs(z-point_list_[i].z_);
            if(cur_distance<min_distance){
                min_distance = cur_distance;
                index = i;
            }
        }
        father_id = point_list_[index].id_;
        father_x = point_list_[index].x_;
        father_z = point_list_[index].z_;

        int step_x;
        int step_z;
        if(abs(x-father_x) > abs(z-father_z)){
            step_x = (x-father_x)>0 ? 1:-1;
            step_x += father_x;
            step_z = father_z;
        }
        else{
            step_x = father_x;
            step_z = (z-father_z)>0 ? 1:-1;
            step_z += father_z;
        }

        if(step_x>=0 && step_x<map_max_x_ && step_z>=0 && step_z<map_max_z_ && valid_world[step_z][step_x]==0){
            Pointcell new_point(step_x, step_z, father_id, number_);
            number_++;
            point_list_.push_back(new_point);
            if(step_x==goal_.x_ && step_z==goal_.z_){
                number_--;
                return true;
            }
        }
        return false;
    }

    void get_path_and_length(){
        path_.clear();
        path_length_ = 0;
        for(int i = number_; i>=0; ){
            path_.push_back(point_list_[i]);
            i = point_list_[i].father_id_;
            path_length_++;
        }
        cout<<"路径长度： "<<path_length_/10.0<<endl;
        cout<<"结点个数： "<<path_.size()<<endl;
        cout<<"搜索次数： "<<iteration_number_<<endl;
    }

};

